using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics
{
	// TODO : Not finished.
	/// <summary>
	/// Defines a collision shape in the form of a triangle.
	/// </summary>
	public class CollisionTriangle : CollisionShape
	{
		/// <summary>
		/// Gets the first vertex of the <see cref="CollisionTriangle"/>.
		/// </summary>
		public Vector3 Position0;

		/// <summary>
		/// Gets the second vertex of the <see cref="CollisionTriangle"/>.
		/// </summary>
		public Vector3 Position1;

		/// <summary>
		/// Gets the third vertex of the <see cref="CollisionTriangle"/>.
		/// </summary>
		public Vector3 Position2;

		/// <summary>
		/// Gets the normal of the <see cref="CollisionTriangle"/>.
		/// </summary>
		/// <remarks>
		/// Do not set this field directly.  Should be considered read-only.
		/// </remarks>
		public Vector3 Normal;

		/// <summary>
		/// Initializes a new instance of the <see cref="CollisionTriangle"/> class.
		/// </summary>
		public CollisionTriangle() : base(CollisionShapeType.Triangle) { }

		#region CollisionShape Members 

		internal override void CalculateInternals()
		{
#if INLINE
			// Calculate the normal of this triangle.
			float x0 = this.Position0.X - this.Position1.X;
			float y0 = this.Position0.Y - this.Position1.Y;
			float z0 = this.Position0.Z - this.Position1.Z;

			float x1 = this.Position0.X - this.Position2.X;
			float y1 = this.Position0.Y - this.Position2.Y;
			float z1 = this.Position0.Z - this.Position2.Z;

			this.Normal.X = (y0 * z1) - (z0 * y1);
			this.Normal.Y = (z0 * x1) - (x0 * z1);
			this.Normal.Z = (x0 * y1) - (y0 * x1);

			float inverseLength = 1.0f / (float)Math.Sqrt(this.Normal.X * this.Normal.X + this.Normal.Y * this.Normal.Y + this.Normal.Z * this.Normal.Z);
			this.Normal.X *= inverseLength;
			this.Normal.Y *= inverseLength;
			this.Normal.Z *= inverseLength;

			// Calculate the distance from the origin.
			//this.distance = -(this.Normal.X * this.Position0.X + this.Normal.Y * this.Position0.Y + this.Normal.Z * this.Position0.Z);
#elif REFERENCE
			// Calculate the normal of this triangle.
			Vector3 edge0, edge1;
			Vector3.Subtract(ref this.Position0, ref this.Position1, out edge0);
			Vector3.Subtract(ref this.Position0, ref this.Position2, out edge1);
			Vector3.Cross(ref edge0, ref edge1, out this.Normal);
			Vector3.Normalize(ref this.Normal, out this.Normal);
			
			// Calculate the distance from the origin.
			//float distance;
			//Vector3.Dot(ref this.Normal, ref this.Position0, out this.distance);
			//this.distance = -this.distance;
#else
			// Calculate the normal of this triangle.
			Vector3 edge0 = this.Position0 - this.Position1;
			Vector3 edge1 = this.Position0 - this.Position2;
			this.Normal = Vector3.Normalize(Vector3.Cross(edge0, edge1));

			// Calculate the distance from the origin.
			//this.distance = -Vector3.Dot(this.Normal, this.Position0);
#endif
			// Call inherited method.
			base.CalculateInternals();
		}

		protected override void CalculateAABB()
		{
			// Calculate the axis-aligned bounding box for the specified triangle.
#if INLINE
			this.AABB.Min.X = Math.Min(Math.Min(this.Position0.X, this.Position1.X), this.Position2.X);
			this.AABB.Min.Y = Math.Min(Math.Min(this.Position0.Y, this.Position1.Y), this.Position2.Y);
			this.AABB.Min.Z = Math.Min(Math.Min(this.Position0.Z, this.Position1.Z), this.Position2.Z);

			this.AABB.Max.X = Math.Max(Math.Max(this.Position0.X, this.Position1.X), this.Position2.X);
			this.AABB.Max.Y = Math.Max(Math.Max(this.Position0.Y, this.Position1.Y), this.Position2.Y);
			this.AABB.Max.Z = Math.Max(Math.Max(this.Position0.Z, this.Position1.Z), this.Position2.Z);
#elif REFERENCE
			Vector3.Min(ref this.Position0, ref this.Position1, out this.AABB.Min);
			Vector3.Min(ref this.AABB.Min, ref this.Position2, out this.AABB.Min);
			Vector3.Max(ref this.Position0, ref this.Position1, out this.AABB.Max);
			Vector3.Max(ref this.AABB.Max, ref this.Position2, out this.AABB.Max);
#else
			this.AABB = new BoundingBox(
				Vector3.Min(this.Position0, Vector3.Min(this.Position1, this.Position2)),
				Vector3.Max(this.Position0, Vector3.Max(this.Position1, this.Position2)));
#endif
		}

		public override bool IntersectsWith(ref CollisionRay ray, out Vector3 position, out Vector3 normal, out float t)
		{
			// Initialize results.
			position = Vector3.Zero;
			normal = Vector3.Zero;
			t = 0.0f;
#if INLINE
			// Get the 1st and 2nd edges.
			Vector3 edge0 = new Vector3()
			{
				X = this.Position1.X - this.Position0.X,
				Y = this.Position1.Y - this.Position0.Y,
				Z = this.Position1.Z - this.Position0.Z
			};
			Vector3 edge1 = new Vector3()
			{
				X = this.Position2.X - this.Position0.X,
				Y = this.Position2.Y - this.Position0.Y,
				Z = this.Position2.Z - this.Position0.Z
			};

			// Get the vector of the collision ray.	
			Vector3 endPoint = ray.GetEndPoint();
			Vector3 vector = new Vector3()
			{
				X = ray.Position.X - endPoint.X,
				Y = ray.Position.Y - endPoint.Y,
				Z = ray.Position.Z - endPoint.Z
			};

			// Calculate the non-unitized triangle normal. 
			Vector3 normal0 = new Vector3()
			{
				X = (edge0.Y * edge1.Z) - (edge0.Z * edge1.Y),
				Y = (edge0.Z * edge1.X) - (edge0.X * edge1.Z),
				Z = (edge0.X * edge1.Y) - (edge0.Y * edge1.X)
			};
			
			// Calculate the denominator and check if the collision ray is pointing towards
			// the triangle.
			float denominator = vector.X * normal0.X + vector.Y * normal0.Y + vector.Z * normal0.Z;
			if (denominator <= 0.0f)
			{
				return false;
			}

			// Calculate intersection with triangle's plane.
			Vector3 difference0 = new Vector3()
			{
				X = ray.Position.X - this.Position0.X,
				Y = ray.Position.Y - this.Position0.Y,
				Z = ray.Position.Z - this.Position0.Z
			};
			t = difference0.X * normal0.X + difference0.Y * normal0.Y + difference0.Z * normal0.Z;
			if (t < 0.0f || t > denominator)
			{
				return false;
			}

			// Calculate the barycentric coordinates and determine if they are within the bounds
			// of the triangle.
			Vector3 e = new Vector3()
			{
				X = (vector.Y * difference0.Z) - (vector.Z * difference0.Y),
				Y = (vector.Z * difference0.X) - (vector.X * difference0.Z),
				Z = (vector.X * difference0.Y) - (vector.Y * difference0.X)
			};

			float v = edge1.X * e.X + edge1.Y * e.Y + edge1.Z * e.Z;
			if (v < 0.0f || v > denominator)
			{
				return false;
			}

			float w = -(edge0.X * e.X + edge0.Y * e.Y + edge0.Z * e.Z);
			if (w < 0.0f || v + w > denominator)
			{
				return false;
			}
#elif REFERENCE
			// Get the 1st and 2nd edges.
			Vector3 edge0, edge1;
			Vector3.Subtract(ref this.Position1, ref this.Position0, out edge0);
			Vector3.Subtract(ref this.Position2, ref this.Position0, out edge1);
			
			// Get the vector of the collision ray.
			Vector3 vector = ray.GetEndPoint();
			Vector3.Subtract(ref ray.Position, ref vector, out vector);
			
			// Calculate the non-unitized triangle normal. 
			Vector3 normal0;
			Vector3.Cross(ref edge0, ref edge1, out normal0);

			// Calculate the denominator and check if the collision ray is pointing towards
			// the triangle.
			float denominator;
			Vector3.Dot(ref vector, ref normal0, out denominator);
			if (denominator <= 0.0f)
			{
				return false;
			}

			// Calculate intersection with triangle's plane.
			Vector3 difference0;
			Vector3.Subtract(ref ray.Position, ref this.Position0, out difference0);
			Vector3.Dot(ref difference0, ref normal0, out t);
			if (t < 0.0f || t > denominator)
			{
				return false;
			}

			// Calculate the barycentric coordinates and determine if they are within the bounds
			// of the triangle.
			Vector3 e;
			Vector3.Cross(ref vector, ref difference0, out e);

			float v;
			Vector3.Dot(ref edge1, ref e, out v);
			if (v < 0.0f || v > denominator)
			{
				return false;
			}

			float w;
			Vector3.Dot(ref edge0, ref e, out w); w = -w;
			if (w < 0.0f || v + w > denominator)
			{
				return false;
			}
#else
			// Get the 1st and 2nd edges.
			Vector3 edge0 = this.Position1 - this.Position0;
			Vector3 edge1 = this.Position2 - this.Position0;

			// Get the vector of the collision ray.			
			Vector3 vector = ray.Position - ray.GetEndPoint();

			// Calculate the non-unitized triangle normal. 
			Vector3 normal0 = Vector3.Cross(edge0, edge1);

			// Calculate the denominator and check if the collision ray is pointing towards
			// the triangle.
			float denominator = Vector3.Dot(vector, normal0);
			if (denominator <= 0.0f)
			{
				return false;
			}

			// Calculate intersection with triangle's plane.
			Vector3 difference0 = ray.Position - this.Position0;
			t = Vector3.Dot(difference0, normal0);
			if (t < 0.0f || t > denominator)
			{
				return false;
			}

			// Calculate the barycentric coordinates and determine if they are within the bounds
			// of the triangle.
			Vector3 e = Vector3.Cross(vector, difference0);

			float v = Vector3.Dot(edge1, e);
			if (v < 0.0f || v > denominator)
			{
				return false;
			}

			float w = -Vector3.Dot(edge0, e);
			if (w < 0.0f || v + w > denominator)
			{
				return false;
			}
#endif
			// Calculate the last barycentric coordinate component.
			float inverseDenominator = 1.0f / denominator;
			t *= inverseDenominator;
			v *= inverseDenominator;
			w *= inverseDenominator;
			//float u = 1.0f - v - w;

			// Determine the position of the intersection using the barycenteric coordinates.
#if INLINE
			position.X = this.Position0.X + edge0.X * v + edge1.X * w;
			position.Y = this.Position0.Y + edge0.Y * v + edge1.Y * w;
			position.Z = this.Position0.Z + edge0.Z * v + edge1.Z * w;
#elif REFERENCE
			Vector3.Multiply(ref edge0, v, out edge0);
			Vector3.Multiply(ref edge1, w, out edge1);
			Vector3.Add(ref edge0, ref edge1, out position);
			Vector3.Add(ref position, ref this.Position0, out position);
#else
			position = this.Position0 + edge0 * v + edge1 * w;
#endif
			normal = this.Normal;

			// An intersection occurred.
			return true;
		}

		protected internal override float GetVolume()
		{
			return 0.0f;
		}

		public override void CalculateMassProperties(float density, out float mass, out Matrix inertiaTensor)
		{
			mass = 0.0f;
			inertiaTensor = MatrixExtension.Zero;
		}

		#endregion
	}
}